'''
Created on Nov 29, 2012

@author: shraybansal
'''
from utils import readAllPickles


def subVec(vec):
    a=0.4
    burtMask = [0.25-a/2, 0.25, a, 0.25, 0.25-a/2]
    for i in range(len(vec)):
        acc_vec = vec[i]
        sub_avec = []
        for j in range(0,len(acc_vec),2):
            n = len(acc_vec)
            smoothFn = 0.0
            sumMask = 0.0
            for k in range(-2,3):
                if(j+k>=0 and j+k<n):
                    smoothFn += burtMask[k+2]*acc_vec[j+k]
                    sumMask+=burtMask[k+2]
            sub_avec.append(smoothFn/sumMask)
        vec[i] = sub_avec
        
    return vec

def smoothVec(vec):
    a=0.4
    burtMask = [0.25-a/2, 0.25, a, 0.25, 0.25-a/2]
    for i in range(len(vec)):
        acc_vec = vec[i]
        sub_avec = []
        for j in range(0,len(acc_vec)):
            n = len(acc_vec)
            smoothFn = 0.0
            sumMask = 0.0
            for k in range(-2,3):
                if(j+k>=0 and j+k<n):
                    smoothFn += burtMask[k+2]*acc_vec[j+k]
                    sumMask+=burtMask[k+2]
            sub_avec.append(smoothFn/sumMask)
        vec[i] = sub_avec
    return vec

def subGes(gest):

    for key in gest.keys():
        accX = subVec(gest[key].returnaX())
        accY = subVec(gest[key].returnaY())
        accZ = subVec(gest[key].returnaZ())
        accNorm = subVec(gest[key].returnaNorm())
        angX = subVec(gest[key].returngX())
        angY = subVec(gest[key].returngY())
        angZ = subVec(gest[key].returngZ())
        worldaX = subVec(gest[key].returnworldaX())
        worldaY = subVec(gest[key].returnworldaY())
        worldaZ = subVec(gest[key].returnworldaZ())
        worldaNorm = subVec(gest[key].returnworldaNorm())
        
        """Set the changed values back to the gesture"""
        gest[key].setFrameAcc(accX, accY, accZ, accNorm)
        gest[key].setAngVel(angX, angY, angZ)
        gest[key].setWrldAcc(worldaX, worldaY, worldaZ, worldaNorm)
        
        
def smoothGes(gest):

    for key in gest.keys():
        accX = smoothVec(gest[key].returnaX())
        accY = smoothVec(gest[key].returnaY())
        accZ = smoothVec(gest[key].returnaZ())
        accNorm = smoothVec(gest[key].returnaNorm())
        angX = smoothVec(gest[key].returngX())
        angY = smoothVec(gest[key].returngY())
        angZ = smoothVec(gest[key].returngZ())
        worldaX = smoothVec(gest[key].returnworldaX())
        worldaY = smoothVec(gest[key].returnworldaY())
        worldaZ = smoothVec(gest[key].returnworldaZ())
        worldaNorm = smoothVec(gest[key].returnworldaNorm())
        
        """Set the changed values back to the gesture"""
        gest[key].setFrameAcc(accX, accY, accZ, accNorm)
        gest[key].setAngVel(angX, angY, angZ)
        gest[key].setWrldAcc(worldaX, worldaY, worldaZ, worldaNorm)
        
        
if __name__=='__main__':
#    vec = [[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10],[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],[1, 1, 10, 1, 1, 1, -1, 1, 1, 1, 1]]
#    a= smoothVec(vec)
#    print a
#    print vec
    
    gestSet = readAllPickles()

    gesN = 'Door_Close'
    accX = gestSet[gesN].returnaX()
    accY = gestSet[gesN].returnaY()
    accZ = gestSet[gesN].returnaZ()
    print gesN, 'X', len(accX[19])
    print gesN, 'Y', len(accY[19])
    print gesN, 'Z', len(accZ[19])
    
    
    smoothGes(gestSet)

    accX = gestSet[gesN].returnaX()
    accY = gestSet[gesN].returnaY()
    accZ = gestSet[gesN].returnaZ()
    print gesN, 'X', len(accX[19])
    print gesN, 'Y', len(accY[19])
    print gesN, 'Z', len(accZ[19])